#include "CTC.h"
#include "stdlib.h"

extern volatile int numSeconds;

uint8_t ackMessage[ACK_MESSAGE_LENGTH] = {ACKNOWLEDGE,0,0};

extern volatile int numSeconds;

RCR_t* createNewRCR(linkID_t linkID, zone_t zone, sleepTime_t sleepTime, battLife_t battLife)
{
	RCR_t* newRCR = (RCR_t*)malloc(sizeof(RCR_t));
	if(newRCR != NULL)
	{
		newRCR -> linkID 			= linkID;
		newRCR -> zone 				= zone;
		newRCR -> ventStatus 		= OPEN;
		newRCR -> sleepTime 		= sleepTime;
		newRCR -> battLife	 		= battLife;
		newRCR -> timeLastSeen		= numSeconds;
	}
	return newRCR;
}

RTS_t* createNewRTS(linkID_t linkID, zone_t zone, sleepTime_t sleepTime, battLife_t battLife)
{
	RTS_t* newRTS = (RTS_t*)malloc(sizeof(RTS_t));
	if(newRTS != NULL)
	{
		newRTS -> linkID 			= linkID;
		newRTS -> zone 				= zone;
		// TODO: set initial temperature?
		newRTS -> previousTemp 		= 75;
		newRTS -> temp			 	= 0;
		newRTS -> integral			= 0;
		newRTS -> sleepTime 		= sleepTime;
		newRTS -> battLife	 		= battLife;
		newRTS -> timeLastSeen		= numSeconds;
	}
	return newRTS;
}

void deleteRCR(RCR_t** RCRarray, uint8_t index)
{
	uint8_t i;
	if(RCRarray[index] != NULL)
	{
		SMPL_Ioctl(IOCTL_OBJ_CONNOBJ, IOCTL_ACT_DELETE, (void*)RCRarray[index] -> linkID);
		for(i = index; i < NUM_RCRS; i++)
		{
			RCRarray[i] = RCRarray[i+1];
		}
	}
}

void deleteRTS(RTS_t** RTSarray, uint8_t index)
{
	uint8_t i;
	if(RTSarray[index] != NULL)
	{
		SMPL_Ioctl(IOCTL_OBJ_CONNOBJ, IOCTL_ACT_DELETE, (void*)RTSarray[index] -> linkID);
		for(i = index; i < NUM_RTSS; i++)
		{
			RTSarray[i] = RTSarray[i+1];
		}
	}
}

void addNewRCR(RCR_t** RCRarray, RCR_t* newRCR)
{
	uint8_t i;
	for(i = 0; i < NUM_RCRS; i++)
	{
		if(RCRarray[i] == NULL)
			break;
	}
	if(newRCR != NULL && i < NUM_RCRS)
			RCRarray[i] = newRCR;
}

void addNewRTS(RTS_t** RTSarray, RTS_t* newRTS)
{
	uint8_t i;
	for(i = 0; i < NUM_RTSS; i++)
	{
		if(RTSarray[i] == NULL)
			break;
	}
	if(newRTS != NULL && i < NUM_RTSS)
			RTSarray[i] = newRTS;
}		

uint8_t isRCR(RCR_t** RCRarray, linkID_t linkID)
{
	uint8_t i;
	for(i = 0; i < NUM_RCRS; i++)
	{
		if(RCRarray[i] == NULL)
			return 0;
		if(RCRarray[i] -> linkID == linkID)
			return 1;
	}
	return 0;
}

uint8_t isRTS(RTS_t** RTSarray, linkID_t linkID)
{
	uint8_t i;
	for(i = 0; i < NUM_RTSS; i++)
	{
		if(RTSarray[i] == NULL)
			return 0;
		if(RTSarray[i] -> linkID == linkID)
			return 1;
	}
	return 0;
}

uint8_t findRCRbyLinkID(RCR_t** RCRarray, linkID_t linkID)
{
	uint8_t i;
	for(i = 0; i < NUM_RCRS; i++)
	{
		if(RCRarray[i] == NULL)
			return NO_DEVICE_FOUND;
		if(RCRarray[i] -> linkID == linkID)
			return i;
	}
	return NO_DEVICE_FOUND;
}

uint8_t findRTSbyLinkID(RTS_t** RTSarray, linkID_t linkID)
{
	uint8_t i;
	for(i = 0; i < NUM_RTSS; i++)
	{
		if(RTSarray[i] == NULL)
			return NO_DEVICE_FOUND;
		if(RTSarray[i] -> linkID == linkID)
			return i;
	}
	return NO_DEVICE_FOUND;
}

// TODO: this only handles one RCR per zone
uint8_t findRCRbyZone(RCR_t** RCRarray, zone_t zone)
{
	uint8_t i;
	for(i = 0; i < NUM_RCRS; i++)
	{
		if(RCRarray[i] == NULL)
			return NO_DEVICE_FOUND;
		if(RCRarray[i] -> zone == zone)
			return i;
	}
	return NO_DEVICE_FOUND;
}

uint8_t findRTSbyZone(RTS_t** RTSarray, zone_t zone)
{
	uint8_t i;
	for(i = 0; i < NUM_RTSS; i++)
	{
		if(RTSarray[i] == NULL)
			return NO_DEVICE_FOUND;
		if(RTSarray[i] -> zone == zone)
			return i;
	}
	return NO_DEVICE_FOUND;
}

void updateRCR(RCR_t** RCRarray, linkID_t linkID, battLife_t battLife)
{
	uint8_t index = findRCRbyLinkID(RCRarray, linkID);
	RCRarray[index] -> battLife = battLife;
	RCRarray[index] -> timeLastSeen = numSeconds;
}

void updateRTS(RTS_t** RTSarray, linkID_t linkID, temp_t temp, battLife_t battLife)
{
	uint8_t index = findRTSbyLinkID(RTSarray, linkID);
	RTSarray[index] -> previousTemp = RTSarray[index] -> temp;
	RTSarray[index] -> temp = temp;
	RTSarray[index] -> battLife = battLife;
	RTSarray[index] -> timeLastSeen = numSeconds;
}

void ackRCR(RCR_t** RCRarray, linkID_t linkID)
{
	uint8_t index = findRCRbyLinkID(RCRarray, linkID);
		
	ackMessage[1] = RCRarray[index] -> sleepTime;
	ackMessage[2] = RCRarray[index] -> ventStatus;
	SMPL_Send(linkID, ackMessage, ACK_MESSAGE_LENGTH);
}

void ackRTS(RTS_t** RTSarray, linkID_t linkID)
{
	uint8_t index = findRTSbyLinkID(RTSarray, linkID);
	ackMessage[1] = RTSarray[index] -> sleepTime;
	SMPL_Send(linkID, ackMessage, ACK_MESSAGE_LENGTH);
}

error_t calculateError(RTS_t** RTSarray, uint8_t index, temp_t setTemp)
{
	float proportional;
	float derivative;
	error_t error;
	
	// if there is no RTS with this index, return 0  
	if(RTSarray[index] == NULL)
		return 0;
	
	// otherwise, calculate the PID error
	proportional = K_P * (RTSarray[index] -> temp - setTemp);
	derivative = K_D * (RTSarray[index] -> temp - RTSarray[index] -> previousTemp)/RTSarray[index] -> sleepTime;
	RTSarray[index] -> integral +=  K_I * (RTSarray[index] -> temp + RTSarray[index] -> previousTemp - 2*setTemp)/2 * RTSarray[index] -> sleepTime;
	
	// if the room is colder than the set temp, let it heat up naturally
	if(setTemp >= RTSarray[index] -> temp)
	{
		// clear integral term since we can't heat the rooms
		RTSarray[index] -> integral = 0;
		return 0;
	}
	
	error = proportional + derivative + RTSarray[index] -> integral;
	RTSarray[index] -> error = error;
	
	return error;
}

error_t sumErrors(RTS_t** RTSarray, temp_t setTemp)
{
	error_t error = 0;
	int i;
	for(i = 0; i < NUM_RTSS; i++)
		error += calculateError(RTSarray, i, setTemp);
	return error;
}

void sortZones(RTS_t** RTSarray, uint8_t* sortedIndices)
{
	int i;
	uint8_t dummy;
	uint8_t swapMade = 1;
	
	for(i = 0; i < NUM_RTSS; i++)
		sortedIndices[i] = i;
	
	while(swapMade)
	{
		swapMade = 0;
		for(i = 0; i < NUM_RTSS; i++)
		{
			if(RTSarray[sortedIndices[i+1]] != NULL)
			{
				if(RTSarray[sortedIndices[i]] -> error < RTSarray[sortedIndices[i+1]] -> error)
				{
					dummy = sortedIndices[i];
					sortedIndices[i] = sortedIndices[i+1];
					sortedIndices[i+1] = dummy;
					swapMade = 1;
				}
			}
		}
	}
}

void openRCR(RCR_t** RCRarray, zone_t zone)
{
	RCRarray[findRCRbyZone(RCRarray, zone)] -> ventStatus = OPEN;
}

void closeRCR(RCR_t** RCRarray, zone_t zone)
{
	RCRarray[findRCRbyZone(RCRarray, zone)] -> ventStatus = CLOSED;
}
